#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include "std_msgs/msg/float32.hpp"

int imax=200;
int i=0;
float angv_zi=0.0;

class ImuDet: public rclcpp::Node
{
private:
    // 声明话题发布者
    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr command_publisher_;
    //声明雷达主题
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
    void imucallback(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(),"imu已接收");
        angv_zi=angv_zi+msg->angular_velocity.z;
        i=i+1;
        if (i==imax)
        {
            // 创建消息
            std_msgs::msg::Float32 angv_z;
            angv_z.data=angv_zi/((float)imax);
            command_publisher_->publish(angv_z);
            RCLCPP_INFO(this->get_logger(),"imu已发送anglar velocity z=%f",angv_z.data);
            i=0;
            angv_zi=0.0;
        }
    }
public:
    ImuDet(std::string name):Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "大家好，我是%s.", name.c_str());
        // 订阅imu主题
        imu_sub_=this->create_subscription<sensor_msgs::msg::Imu>(
            "imu/data_raw",10,std::bind(&ImuDet::imucallback,this,std::placeholders::_1));
        // 创建发布者
        command_publisher_=this->create_publisher<std_msgs::msg::Float32>(
            "imu_angv_z", 10);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<ImuDet>("imu_det");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}